
// import haxe.ds.Array;
// import oimo.common.Setting;
// import oimo.dynamics.TimeStep;
// import oimo.dynamics.constraint.ConstraintSolver;
// import oimo.dynamics.constraint.info.JacobianRow;
// import oimo.dynamics.constraint.info.joint.JointSolverInfo;
// import oimo.dynamics.constraint.info.joint.JointSolverInfoRow;
// import oimo.dynamics.constraint.joint.Joint;
// import oimo.dynamics.constraint.joint.JointImpulse;
// import oimo.dynamics.constraint.solver.common.JointSolverMassDataRow;
// import oimo.m.Vec3;
// import oimo.m.M;

import { ConstraintSolver } from "../../ConstraintSolver";
import { JointSolverInfo } from "../../info/joint/JointSolverInfo";
import { JointSolverMassDataRow } from "../common/JointSolverMassDataRow";
import { MassMatrix } from "./MassMatrix";
import { BoundaryBuilder } from "./BoundaryBuilder";
import { BoundarySelector } from "./BoundarySelector";
import { Setting } from "../../../../common/Setting";
import { Vec3 } from "../../../../common/Vec3";
import { M } from "../../../../common/M";
import { JointSolverInfoRow } from "../../info/joint/JointSolverInfoRow";
import { JacobianRow } from "../../info/JacobianRow";
import { TimeStep } from "../../../TimeStep";
import { JointImpulse } from "../../joint/JointImpulse";
import { Boundary } from "./Boundary";
import { Joint } from "../../joint/Joint";

/**
 * The direct solver of a mixed linear complementality problem (MLCP) for
 * joint constraints.
 */
export class DirectJointConstraintSolver extends ConstraintSolver {
	info: JointSolverInfo;
	massData: Array<JointSolverMassDataRow>;

	relVels: Array<number>;
	impulses: Array<number>;
	dImpulses: Array<number>;
	dTotalImpulses: Array<number>;

	joint: Joint;

	massMatrix: MassMatrix;

	boundaryBuilder: BoundaryBuilder;

	velBoundarySelector: BoundarySelector;
	posBoundarySelector: BoundarySelector;


	constructor(joint: Joint) {
		super();
		this.joint = joint;
		this.info = new JointSolverInfo();

		var maxRows: number = Setting.maxJacobianRows;

		this.massMatrix = new MassMatrix(maxRows);
		this.boundaryBuilder = new BoundaryBuilder(maxRows);
		this.massData = new Array<JointSolverMassDataRow>(maxRows);
		for (let i = 0; i < this.massData.length; ++i) {
			this.massData[i] = new JointSolverMassDataRow();
		}

		var numMaxBoundaries: number = this.boundaryBuilder.boundaries.length;
		this.velBoundarySelector = new BoundarySelector(numMaxBoundaries);
		this.posBoundarySelector = new BoundarySelector(numMaxBoundaries);

		this.relVels = new Array<number>(maxRows);
		this.impulses = new Array<number>(maxRows);
		this.dImpulses = new Array<number>(maxRows);
		this.dTotalImpulses = new Array<number>(maxRows);

		for (let i = 0; i < maxRows; ++i) {
			this.relVels[i] = 0;
			this.impulses[i] = 0;
			this.dImpulses[i] = 0;
			this.dTotalImpulses[i] = 0;
		}
	}


	private applyImpulses(impulses: Array<number>): void {
		var linearSet: boolean = false;
		var angularSet: boolean = false;
		var lv1: Vec3 = this._b1._vel.clone();
		var lv2: Vec3 = this._b2._vel.clone();
		var av1: Vec3 = this._b1._angVel.clone();
		var av2: Vec3 = this._b2._angVel.clone();

		for (let i = 0; i < this.info.numRows; ++i) {
			var row: JointSolverInfoRow = this.info.rows[i];
			var j: JacobianRow = row.jacobian;
			var md: JointSolverMassDataRow = this.massData[i];
			var imp: number = impulses[i];
			if (j.isLinearSet()) {
				M.vec3_addRhsScaledEQ(lv1, lv1, md.invMLin1, imp);
				M.vec3_addRhsScaledEQ(lv2, lv2, md.invMLin2, -imp);
				linearSet = true;
			}
			if (j.isAngularSet()) {
				M.vec3_addRhsScaledEQ(av1, av1, md.invMAng1, imp);
				M.vec3_addRhsScaledEQ(av2, av2, md.invMAng2, -imp);
				angularSet = true;
			}
		}
		if (linearSet) {
			M.vec3_assign(this._b1._vel, lv1);
			M.vec3_assign(this._b2._vel, lv2);
		}
		if (angularSet) {
			M.vec3_assign(this._b1._angVel, av1);
			M.vec3_assign(this._b2._angVel, av2);
		}
	}


	private applySplitImpulses(impulses: Array<number>): void {
		var linearSet: boolean = false;
		var angularSet: boolean = false;
		var lv1: Vec3 = this._b1._pseudoVel.clone();
		var lv2: Vec3 = this._b2._pseudoVel.clone();
		var av1: Vec3 = this._b1._angPseudoVel.clone();
		var av2: Vec3 = this._b2._angPseudoVel.clone();

		for (let i = 0; i < this.info.numRows; ++i) {
			var row: JointSolverInfoRow = this.info.rows[i];
			var j: JacobianRow = row.jacobian;
			var md: JointSolverMassDataRow = this.massData[i];
			var imp: number = impulses[i];
			if (j.isLinearSet()) {
				M.vec3_addRhsScaledEQ(lv1, lv1, md.invMLin1, imp);
				M.vec3_addRhsScaledEQ(lv2, lv2, md.invMLin2, -imp);
				linearSet = true;
			}
			if (j.isAngularSet()) {
				M.vec3_addRhsScaledEQ(av1, av1, md.invMAng1, imp);
				M.vec3_addRhsScaledEQ(av2, av2, md.invMAng2, -imp);
				angularSet = true;
			}
		}
		if (linearSet) {
			M.vec3_assign(this._b1._pseudoVel, lv1);
			M.vec3_assign(this._b2._pseudoVel, lv2);
		}
		if (angularSet) {
			M.vec3_assign(this._b1._angPseudoVel, av1);
			M.vec3_assign(this._b2._angPseudoVel, av2);
		}
	}


	private applyPositionImpulses(impulses: Array<number>): void {
		var linearSet: boolean = false;
		var angularSet: boolean = false;
		var lv1: Vec3 = new Vec3();
		var lv2: Vec3 = new Vec3();
		var av1: Vec3 = new Vec3();
		var av2: Vec3 = new Vec3();

		for (let i = 0; i < this.info.numRows; ++i) {
			var row: JointSolverInfoRow = this.info.rows[i];
			var j: JacobianRow = row.jacobian;
			var md: JointSolverMassDataRow = this.massData[i];
			var imp: number = impulses[i];
			if (j.isLinearSet()) {
				M.vec3_addRhsScaledEQ(lv1, lv1, md.invMLin1, imp);
				M.vec3_addRhsScaledEQ(lv2, lv2, md.invMLin2, -imp);
				linearSet = true;
			}
			if (j.isAngularSet()) {
				M.vec3_addRhsScaledEQ(av1, av1, md.invMAng1, imp);
				M.vec3_addRhsScaledEQ(av2, av2, md.invMAng2, -imp);
				angularSet = true;
			}
		}
		if (linearSet) {
			(this._b1._applyTranslation(lv1));
			(this._b2._applyTranslation(lv2));
		}
		if (angularSet) {
			(this._b1._applyRotation(av1));
			(this._b2._applyRotation(av2));
		}
	}

	public preSolveVelocity(timeStep: TimeStep): void {
		this.joint._syncAnchors();
		this.joint._getVelocitySolverInfo(timeStep, this.info);

		this._b1 = this.info.b1;
		this._b2 = this.info.b2;

		// compute inverse mass matrix
		this.massMatrix.computeInvMass(this.info, this.massData);

		// build boundaries
		this.boundaryBuilder.buildBoundaries(this.info);

		// update the size of the boundary selector
		this.velBoundarySelector.setSize(this.boundaryBuilder.numBoundaries);
	}

	public warmStart(timeStep: TimeStep): void {
		var factor: number = this.joint._getWarmStartingFactor();

		// adjust impulse for variable time step
		factor *= timeStep.dtRatio;

		// warm start disabled
		if (factor <= 0) {
			for (let i = 0; i < this.info.numRows; ++i) {
				var row: JointSolverInfoRow = this.info.rows[i];
				row.impulse.clear();
			}
			return;
		}

		for (let i = 0; i < this.info.numRows; ++i) {
			var row: JointSolverInfoRow = this.info.rows[i];
			var imp: JointImpulse = row.impulse;

			// update limit impulse
			var impulse: number = imp.impulse * factor;
			if (impulse < row.minImpulse) impulse = row.minImpulse;
			else if (impulse > row.maxImpulse) impulse = row.maxImpulse;
			imp.impulse = impulse;

			if (row.motorMaxImpulse > 0) {
				var impulseM: number = imp.impulseM * factor;
				var max: number = row.motorMaxImpulse;
				if (impulseM < -max) impulseM = -max;
				else if (impulseM > max) impulseM = max;
				imp.impulseM = impulseM;
			} else {
				imp.impulseM = 0;
			}

			this.dImpulses[i] = imp.impulse + imp.impulseM;
		}
		// apply initial impulse
		this.applyImpulses(this.dImpulses);
	}

	public solveVelocity(): void {
		var numRows: number = this.info.numRows;
		var lv1: Vec3 = this._b1._vel.clone();
		var lv2: Vec3 = this._b2._vel.clone();
		var av1: Vec3 = this._b1._angVel.clone();
		var av2: Vec3 = this._b2._angVel.clone();

		for (let i = 0; i < numRows; ++i) {
			var row: JointSolverInfoRow = this.info.rows[i];
			var imp: JointImpulse = row.impulse;
			var j: JacobianRow = row.jacobian;

			// compute relative velocity
			var relVel: number = 0;
			relVel += M.vec3_dot(lv1, j.lin1);
			relVel -= M.vec3_dot(lv2, j.lin2);
			relVel += M.vec3_dot(av1, j.ang1);
			relVel -= M.vec3_dot(av2, j.ang2);
			this.relVels[i] = relVel;

			// get impulse
			this.impulses[i] = imp.impulse;

			// clear total impulse
			this.dTotalImpulses[i] = 0;
		}

		// solve motors first
		var invMass: Array<Array<number>> = this.massMatrix._invMassWithoutCfm;
		for (let i = 0; i < numRows; ++i) {
			var row: JointSolverInfoRow = this.info.rows[i];
			var imp: JointImpulse = row.impulse;
			var md: JointSolverMassDataRow = this.massData[i];
			if (row.motorMaxImpulse > 0) {
				// relative velocity : body1 - body2
				// motor speed       : body2 - body1
				//  =>
				// target relative velocity : -[motor speed]
				var oldImpulseM: number = imp.impulseM;
				var impulseM: number = oldImpulseM + md.massWithoutCfm * (-row.motorSpeed - this.relVels[i]);

				// clamp motor impulse
				var maxImpulseM: number = imp.impulseM;
				if (impulseM < -maxImpulseM) impulseM = -maxImpulseM;
				else if (impulseM > maxImpulseM) impulseM = maxImpulseM;
				imp.impulseM = impulseM;

				// compute delta motor impulse
				var dImpulseM: number = impulseM - oldImpulseM;
				this.dTotalImpulses[i] = dImpulseM;

				// update relative velocity (apply the delta impulse virtually)
				for (let j = 0; j < numRows; ++j) {
					this.relVels[j] += dImpulseM * invMass[i][j];
				}
			}
		}

		// try all the boundaries
		var solved: boolean = false;
		for (let i = 0; i < this.boundaryBuilder.numBoundaries; ++i) {
			// select a boundary
			var idx: number = this.velBoundarySelector.getIndex(i);
			var b: Boundary = this.boundaryBuilder.boundaries[idx];

			// try the case
			if (b.computeImpulses(this.info, this.massMatrix, this.relVels, this.impulses, this.dImpulses, 1, false)) {
				// the solution found
				for (let j = 0; j < numRows; ++j) {
					var row: JointSolverInfoRow = this.info.rows[j];
					var imp: JointImpulse = row.impulse;
					var dimp: number = this.dImpulses[j];

					// accumulate the delta impulses
					imp.impulse += dimp;
					this.dTotalImpulses[j] += dimp;
				}

				// apply motor + limit impulses
				this.applyImpulses(this.dTotalImpulses);

				// make the priority of the boundary higher for the next iteration
				this.velBoundarySelector.select(idx);
				solved = true;
				break;
			}
		}

		if (!solved) {
			console.error("could not find solution. (velocity)");
			return;
		}
	}

	public postSolveVelocity(timeStep: TimeStep): void {
		// compute total linear and angular impulse
		var lin: Vec3 = new Vec3();
		var ang: Vec3 = new Vec3();

		for (let i = 0; i < this.info.numRows; ++i) {
			var row: JointSolverInfoRow = this.info.rows[i];
			var imp: JointImpulse = row.impulse;
			var j: JacobianRow = row.jacobian;
			if (j.isLinearSet()) {
				// assume that this row is linear
				M.vec3_addRhsScaledEQ(lin, lin, j.lin1, imp.impulse);
			} else if (j.isAngularSet()) {
				// assume that this row is angular
				M.vec3_addRhsScaledEQ(ang, ang, j.ang1, imp.impulse);
			}
		}

		M.vec3_scale(this.joint._appliedForce, lin, timeStep.invDt);
		M.vec3_scale(this.joint._appliedTorque, ang, timeStep.invDt);
	}


	private updatePositionData(): void {
		this.joint._syncAnchors();
		this.joint._getPositionSolverInfo(this.info);

		this._b1 = this.info.b1;
		this._b2 = this.info.b2;

		// compute inverse mass matrix
		this.massMatrix.computeInvMass(this.info, this.massData);

		// build boundaries
		this.boundaryBuilder.buildBoundaries(this.info);

		// update the size of the boundary selector
		this.posBoundarySelector.setSize(this.boundaryBuilder.numBoundaries);
	}

	public preSolvePosition(timeStep: TimeStep): void {
		this.updatePositionData();

		// clear position impulses
		for (let i = 0; i < this.info.numRows; ++i) {
			this.info.rows[i].impulse.impulseP = 0;
		}
	}

	public solvePositionSplitImpulse(): void {
		var numRows: number = this.info.numRows;
		var lv1: Vec3 = this._b1._pseudoVel.clone();
		var lv2: Vec3 = this._b2._pseudoVel.clone();
		var av1: Vec3 = this._b1._angPseudoVel.clone();
		var av2: Vec3 = this._b2._angPseudoVel.clone();

		for (let i = 0; i < numRows; ++i) {
			var row: JointSolverInfoRow = this.info.rows[i];
			var imp: JointImpulse = row.impulse;
			var j: JacobianRow = row.jacobian;

			// compute relative velocity
			var relVel: number = 0;
			relVel += M.vec3_dot(lv1, j.lin1);
			relVel -= M.vec3_dot(lv2, j.lin2);
			relVel += M.vec3_dot(av1, j.ang1);
			relVel -= M.vec3_dot(av2, j.ang2);
			this.relVels[i] = relVel;

			//trace('pre relVels[$i] = ${relVels[i]}');

			// get impulse
			this.impulses[i] = imp.impulseP;
		}

		// try all the boundaries
		var solved: boolean = false;
		for (let i = 0; i < this.boundaryBuilder.numBoundaries; ++i) {
			// select a boundary
			var idx: number = this.posBoundarySelector.getIndex(i);
			var b: Boundary = this.boundaryBuilder.boundaries[idx];

			// try the case
			if (b.computeImpulses(this.info, this.massMatrix, this.relVels, this.impulses, this.dImpulses, Setting.positionSplitImpulseBaumgarte, false)) {
				// the solution found
				for (let j = 0; j < numRows; ++j) {
					var row: JointSolverInfoRow = this.info.rows[j];
					var imp: JointImpulse = row.impulse;
					var dimp: number = this.dImpulses[j];

					// accumulate the delta impulses
					imp.impulseP += dimp;
				}

				// apply delta impulses
				this.applySplitImpulses(this.dImpulses);

				// make the priority of the boundary higher for the next iteration
				this.posBoundarySelector.select(idx);
				solved = true;
				break;
			}
		}

		if (!solved) {
			console.error("could not find solution. (split impulse)");
			return;
		}
	}

	public solvePositionNgs(timeStep: TimeStep): void {
		this.updatePositionData();

		var numRows: number = this.info.numRows;

		for (let i = 0; i < numRows; ++i) {
			var row: JointSolverInfoRow = this.info.rows[i];
			var imp: JointImpulse = row.impulse;
			var j: JacobianRow = row.jacobian;

			// set relative velocity zero for NGS
			this.relVels[i] = 0;

			// get impulse
			this.impulses[i] = imp.impulseP;
		}

		// try all the boundaries
		var solved: boolean = false;
		for (let i = 0; i < this.boundaryBuilder.numBoundaries; ++i) {
			// select a boundary
			var idx: number = this.posBoundarySelector.getIndex(i);
			var b: Boundary = this.boundaryBuilder.boundaries[idx];

			// try the case
			if (b.computeImpulses(this.info, this.massMatrix, this.relVels, this.impulses, this.dImpulses, Setting.positionNgsBaumgarte, false)) {
				// the solution found
				for (let j = 0; j < numRows; ++j) {
					var row: JointSolverInfoRow = this.info.rows[j];
					var imp: JointImpulse = row.impulse;
					var dimp: number = this.dImpulses[j];

					// accumulate the delta impulses
					imp.impulseP += dimp;
				}

				// apply delta impulses
				this.applyPositionImpulses(this.dImpulses);

				// make the priority of the boundary higher for the next iteration
				this.posBoundarySelector.select(idx);
				solved = true;
				break;
			}
		}

		if (!solved) {
			console.error("could not find solution. (NGS)");
			return;
		}
	}

	public postSolve(): void {
		this.joint._syncAnchors();
		this.joint._checkDestruction();
	}

}
